#include <iostream>
#include <cmath>

double degressToRadians(double degrees) {
    return degrees * M_PI / 180;
}

double radiansToDegrees(double radians) {
    return radians * 180 / M_PI;
}

// 计算两个向量的夹角(考虑正负方向)
double angleDifferenceWithSign(double yaw1, double yaw2){
    // 将yaw角度转换为弧度
    double yaw1_radians = degressToRadians(yaw1);
    double yaw2_radians = degressToRadians(yaw2);

    // 将yaw角转换为二维向量
    double x1 = std::cos(yaw1_radians), y1 = std::sin(yaw1_radians);
    double x2 = std::cos(yaw2_radians), y2 = std::sin(yaw2_radians);

    // 计算夹角的正弦和余弦值
    double sin_theta = x1 * y2 - x2 * y1;
    double cos_theta = x1 * x2 + y1 * y2;

    // 使用atan2计算夹角（考虑正负方向）
    double angle_rad = std::atan2(sin_theta, cos_theta);

    // 将弧度转换为角度
    double angle_deg = radiansToDegrees(angle_rad);

    return angle_deg;

}

int main(){
    double yaw1, yaw2;
    std:: cout << "Please input yaw1: ";
    std:: cin >> yaw1;
    std:: cout << "Please input yaw2: ";
    std:: cin >> yaw2;

    double diff = angleDifferenceWithSign(yaw1, yaw2);
    std:: cout << "The angle difference between yaw1 and yaw2 is: " << diff << std::endl;
}